package com.yhy.wxzn.wcs.device.rgv.core.pathplanning;

import cn.hutool.core.collection.CollectionUtil;
import com.yhy.wxzn.wcs.device.rgv.core.lock.LockNodeManager;
import com.yhy.wxzn.wcs.device.rgv.bean.*;
import com.yhy.wxzn.wcs.device.rgv.enums.NodeStatus;
import com.yhy.wxzn.wcs.device.rgv.enums.NodeType;
import com.yhy.wxzn.wcs.device.rgv.enums.RgvStatus;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.BeanUtils;

import java.util.*;
import java.util.stream.Collectors;



@Slf4j
public class PathPlaningHandlerImpl implements PathPlanningHandler {


    private Queue<Node> open = new PriorityQueue<>();
    private Set<Node> close = new HashSet<>();

    private HashMap<Node,Node> map;



    public PathPlaningHandlerImpl(List<Node> map){
        this.map = new HashMap(Math.max((int) (map.size()/.75f) + 1, 16));
        map.forEach(m->this.map.put(m,m));
    }


    public static void main(String[] args) {

        sameLayerTest();

        diffLayerTest();



//        Node n1 = new Node(new Coord(1, 1, 1), NodeType.P, NodeStatus.Y,true,true,true,true);
//        Node n2 = new Node(new Coord(1, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n3 = new Node(new Coord(1, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n4 = new Node(new Coord(1, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n5 = new Node(new Coord(2, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n6 = new Node(new Coord(2, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
//        Node n7 = new Node(new Coord(2, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
//        Node n8 = new Node(new Coord(2, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n9 = new Node(new Coord(3, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n10 = new Node(new Coord(3, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n11 = new Node(new Coord(3, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n12 = new Node(new Coord(3, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n13 = new Node(new Coord(4, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n14 = new Node(new Coord(4, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
//        Node n15 = new Node(new Coord(4, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
//        Node n16 = new Node(new Coord(4, 4, 1),NodeType.B,NodeStatus.N,true,true,true,true);
//        Node n17 = new Node(new Coord(5, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n18 = new Node(new Coord(5, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n19 = new Node(new Coord(5, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//        Node n20 = new Node(new Coord(5, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
//
//
//        List<Fragment> fragments = Fragment.buildFragments(Arrays.asList(n5,n6,n7,n11,n10));
//
//
//        System.out.printf("");



    }

    private static void sameLayerTest() {
        Node n1 = new Node(new Coord(1, 1, 1), NodeType.P, NodeStatus.Y,true,true,true,true);
        Node n2 = new Node(new Coord(1, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n3 = new Node(new Coord(1, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n4 = new Node(new Coord(1, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n5 = new Node(new Coord(2, 1, 1),NodeType.B,NodeStatus.Y,true,true,true,true);
        Node n6 = new Node(new Coord(2, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n7 = new Node(new Coord(2, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n8 = new Node(new Coord(2, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n9 = new Node(new Coord(3, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n10 = new Node(new Coord(3, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n11 = new Node(new Coord(3, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n12 = new Node(new Coord(3, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n13 = new Node(new Coord(4, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n14 = new Node(new Coord(4, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n15 = new Node(new Coord(4, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n16 = new Node(new Coord(4, 4, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n17 = new Node(new Coord(5, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n18 = new Node(new Coord(5, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n19 = new Node(new Coord(5, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n20 = new Node(new Coord(5, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);

        List<Node> nodes = Arrays.asList(n1, n2, n3, n4,n5,n6,n7,n8,n9,n10,n11,n12,n13,n14,n15,n16,n17,n18,n19,n20);
        List<Route> routes = new PathPlaningHandlerImpl(nodes).calculatePath(n1, n20, false);

        routes.get(0).getPath().forEach(n->System.out.println(n.toString()));
        System.out.println(routes.get(0).getCost());
    }

    private static void diffLayerTest() {
        Node n1 = new Node(new Coord(1, 1, 1), NodeType.L, NodeStatus.Y,true,true,true,true);
        Node n2 = new Node(new Coord(1, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n3 = new Node(new Coord(1, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n4 = new Node(new Coord(1, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n5 = new Node(new Coord(2, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n6 = new Node(new Coord(2, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n7 = new Node(new Coord(2, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n8 = new Node(new Coord(2, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n9 = new Node(new Coord(3, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n10 = new Node(new Coord(3, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n11 = new Node(new Coord(3, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n12 = new Node(new Coord(3, 4, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n13 = new Node(new Coord(4, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n14 = new Node(new Coord(4, 2, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n15 = new Node(new Coord(4, 3, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n16 = new Node(new Coord(4, 4, 1),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n17 = new Node(new Coord(5, 1, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n18 = new Node(new Coord(5, 2, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n19 = new Node(new Coord(5, 3, 1),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n20 = new Node(new Coord(5, 4, 1),NodeType.L,NodeStatus.Y,true,true,true,true);


        Node n21 = new Node(new Coord(1, 1, 2), NodeType.L, NodeStatus.Y,true,true,true,true);
        Node n22 = new Node(new Coord(1, 2, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n23 = new Node(new Coord(1, 3, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n24 = new Node(new Coord(1, 4, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n25 = new Node(new Coord(2, 1, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n26 = new Node(new Coord(2, 2, 2),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n27 = new Node(new Coord(2, 3, 2),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n28 = new Node(new Coord(2, 4, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n29 = new Node(new Coord(3, 1, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n30 = new Node(new Coord(3, 2, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n31 = new Node(new Coord(3, 3, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n32 = new Node(new Coord(3, 4, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n33 = new Node(new Coord(4, 1, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n34 = new Node(new Coord(4, 2, 2),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n35 = new Node(new Coord(4, 3, 2),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n36 = new Node(new Coord(4, 4, 2),NodeType.B,NodeStatus.N,true,true,true,true);
        Node n37 = new Node(new Coord(5, 1, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n38 = new Node(new Coord(5, 2, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n39 = new Node(new Coord(5, 3, 2),NodeType.P,NodeStatus.Y,true,true,true,true);
        Node n40 = new Node(new Coord(5, 4, 2),NodeType.L,NodeStatus.Y,true,true,true,true);

        List<Node> nodes = Arrays.asList(n1, n2, n3, n4,n5,n6,n7,n8,n9,n10,n11,n12,n13,n14,n15,n16,n17,n18,n19,n20 ,n21,n22 ,n23 ,n24, n25 ,n26 ,n27 ,n28 ,n29 ,n30 ,n31 ,n32 ,n33 ,n34 ,n35 ,n36 ,n37 ,n38 ,n39 ,n40);
        List<Route> routes = new PathPlaningHandlerImpl(nodes).calculatePath(n9, n29, false);


        System.out.println("diffLayerTest()");
        for (Route route : routes) {
            System.out.println("=======================================================");
            route.getPath().forEach(n->System.out.println(n.toString()));
            System.out.println(route.getCost());
        }




        List<Route> routes2 = new PathPlaningHandlerImpl(nodes).calculatePath(n18, n37, false);
        System.out.println("diffLayerTest()");
        for (Route route : routes2) {
            System.out.println("=======================================================");
            route.getPath().forEach(n->System.out.println(n.toString()));
            System.out.println(route.getCost());
        }



    }


    /**
     *  map中数据在保证不修改的情况下，路径规划方法可重复使用,该方法是线程不安全的
     * @param start
     * @param end
     * @param palletStatus
     * @return
     */
    @Override
    public List<Route> calculatePath(Node start,Node end,boolean palletStatus) {
        return start.getZ() == end.getZ() ? sameLayerPath(start,end,palletStatus) : differentLayerPath(start,end,palletStatus);
    }

    private List<Route> differentLayerPath(Node start, Node end, boolean palletStatus) {

        //换层路径规划
        Map<Lifter, List<Node>> lifterListMap = map.keySet().stream().
                filter(n -> Objects.equals(NodeType.L, n.getNodeType())
                        && (n.getZ() == start.getZ() || n.getZ() == end.getZ())
                ).collect(Collectors.groupingBy(n -> new Lifter(n)));

        if (CollectionUtil.isEmpty(lifterListMap)){
            throw new RuntimeException(String.format("%s到%s无可用提升机1",start.toString(),end.toString()));
        }

        //算出每个从起点到提升机点，提升机点到终点的消耗值，取最小的消耗值得routes
        int minCost = Integer.MAX_VALUE;
        Lifter minLifer = null;
        for (List<Node> liferNodes : lifterListMap.values()) {
            if (liferNodes.size() != 2) continue;
            int cost = differentLayerPath(start,end,liferNodes,palletStatus);
            if (minCost >  cost){
                minCost =  cost;
                minLifer = new Lifter(liferNodes.get(0).getX(),liferNodes.get(0).getY());
            }
        }

        if (minLifer == null){
            throw new RuntimeException(String.format("%s到%s无可用提升机2",start.toString(),end.toString()));
        }

        return Arrays.asList(
                pathPlanning(start,new Node(minLifer.getX(),minLifer.getY(),start.getZ()),palletStatus,true)
                ,pathPlanning(new Node(minLifer.getX(),minLifer.getY(),end.getZ()),end,palletStatus,true)
        );
    }

    private int differentLayerPath(Node start,Node end,List<Node> sameLiferNodes,boolean palletStatus) {
        Node n1 = sameLiferNodes.get(0);
        Node n2 = sameLiferNodes.get(1);
        boolean error = false;
        Route route1 = null;
        Route route2 = null;
        try {
             route1 = pathPlanning(start, start.getZ() == n1.getZ() ? n1 : n2, palletStatus, true);
             route2 = pathPlanning(end.getZ() == n1.getZ() ? n1: n2, end, palletStatus, true);
        }catch (Exception e){
            log.info(e.getMessage());
            error = true;
        }

        return error ? Integer.MAX_VALUE : route1.getCost() + route2.getCost();
    }

    private Route pathPlanning(Node start,Node end,boolean palletStatus,boolean clearData){
        start = getNodeInMap(start);
        end = getNodeInMap(end);
        boolean flag = false;
        // 开始搜索
        open.add(start);
        while (!open.isEmpty()) {
            Node current = open.poll();
            close.add(current);
            addNeighborNodeInOpen(current,end,palletStatus);
            if (isEndInClose(end)) {
                flag = true;
                break;
            }
        }

        if (!flag){
            throw new RuntimeException("请检查"+start.toString()+"到"+end.toString()+"能否通行");
        }
        Route route = new Route();
        route.setStart(start);
        route.setEnd(end);
        route.setPath(drawPath(end));
        route.setCost(end.getG());
        if (clearData) clearData();
        return route;
    }

    private Node getNodeInMap(Node node) {
        Node s = map.get(node);
        Node t = new Node();
        BeanUtils.copyProperties(s,t);
        return t;
    }

    private void clearData() {
        open.clear();
        close.clear();
    }


    private List<Route> sameLayerPath(Node start,Node end,boolean palletStatus){
      return Arrays.asList(this.pathPlanning(start,end,palletStatus,false));
    }




    /**
     * 判断结点是否是最终结点
     */
    private boolean isEndNode(Coord end,Coord coord) {
        return  end.equals(coord);
    }



    /**
     * 判断结点能否放入Open列表
     */
    private boolean canAddNodeToOpen( Node node,boolean palletStatus) {
        //节点是禁用
        if (Objects.equals(node.getNodeType(),NodeType.B))
            return false;


        // 判断当前是否是载货行驶，并该节点状态为有货状态
        if (palletStatus && Objects.equals(node.getNodeStatus(),NodeStatus.Y) && !node.getNodeType().equals(NodeType.CONNECTION))
            return false;

        //状态错误的小车不能通行
        if (LockNodeManager.checkNodesLocked(node)){
            Object lockNodeObj = LockNodeManager.getLockNodeObj(node);
            if (lockNodeObj != null && lockNodeObj instanceof FourWayRgv){
                FourWayRgv fourWayRgv = (FourWayRgv) lockNodeObj;
                if (fourWayRgv.getRgvStatus() == RgvStatus.ERROR)
                    return false;
            }
        }


        // 判断结点是否存在close表
        if (isCoordInClose(node))
            return false;


        return true;
    }

    /**
     * 判断坐标是否在close表中
     */
    private boolean isEndInClose(Node end) {
        return close.contains(end);
    }

    private boolean isCoordInClose(Node curNode) {
        return close.contains(curNode);
    }



    private int calcH(Coord end,Coord coord) {
        return Math.abs(end.getX() - coord.getX()) + Math.abs(end.getY() - coord.getY());
    }


    private Node findNodeInOpen(Node node) {
        for (Node o : open) {
            if (o.equals(node))
                return o;
        }
        return null;
    }

    /**
     * 添加所有邻结点到open表
     */
    private void addNeighborNodeInOpen(Node curNode,Node end,boolean palletStatus) {
        int x = curNode.getX();
        int y = curNode.getY();
        int z = curNode.getZ();

        // 上
        if (curNode.getUp()){
            addNeighborNodeInOpen(curNode, new Node(x - 1,y, z),end, palletStatus,Node.COST);
        }

        // 下
        if (curNode.getDown()){
            addNeighborNodeInOpen(curNode, new Node(x + 1,y, z),end, palletStatus, Node.COST);
        }
        // 左
        if (curNode.getLeft()){
            addNeighborNodeInOpen(curNode, new Node(x, y - 1, z),end, palletStatus, Node.COST);
        }

        // 右
        if (curNode.getRight()){
            addNeighborNodeInOpen(curNode, new Node(x, y + 1, z),end, palletStatus, Node.COST);
        }
    }

    /**
     * 添加一个邻结点到open表
     */
    private void addNeighborNodeInOpen(Node current, Node next ,Node end,boolean palletStatus,int value) {
        if ((next = checkNodeInMap(next)) == null)
            return;

        if (canAddNodeToOpen(next,palletStatus)) {
            int G = current.getG() + value; // 计算邻结点的G值
            Node oldNextInOpen = findNodeInOpen(next);
            if (oldNextInOpen == null) {
                int H=calcH(end,next); // 计算H值
                if (isEndNode(end,next)){
                    next = end;
                }
                next.setH(H);
                next.setG(G);
                next.setParent(current);
                open.add(next);
            }else if (oldNextInOpen.getG() > G){
                oldNextInOpen.setParent(current);
                oldNextInOpen.setG(G);
                open.add(oldNextInOpen);
            }
        }
    }

    private Node checkNodeInMap(Node next) {
        if (map.containsKey(next)){
            BeanUtils.copyProperties(map.get(next), next);
            return next;
        }

        return null;
    }

    private List<Node> drawPath(Node end) {

        List<Node> resultPath = new ArrayList<>();

        Node cur = end;
        while (cur != null) {
            resultPath.add(cur);
            cur = cur.getParent();
        }

        Collections.reverse(resultPath);

        return resultPath;
    }


}
